#include "ros/ros.h"
#include "sensor_msgs/Imu.h"
#include "tf/tf.h"

void callback(const sensor_msgs::Imu msg)
{
    /* 检测消息包中四元数数据是否存在 */
    if (msg.orientation_covariance[0] < 0)
        return;

    /* 取出 IMU 消息中的四元数姿态信息 */
    tf::Quaternion quaternion(
        msg.orientation.x,
        msg.orientation.y,
        msg.orientation.z,
        msg.orientation.w);

    /* 将四元数转化为欧拉角 */
    double roll, pitch, yaw;
    tf::Matrix3x3(quaternion).getRPY(roll, pitch, yaw);

    /* 弧度换算成角度 */
    roll = roll * 180 / M_PI;
    pitch = pitch * 180 / M_PI;
    yaw = yaw * 180 / M_PI;
    ROS_INFO("roll = %.0f pitch = %.0f yaw = %.0f", roll, pitch, yaw);
}

int main(int argc, char **argv)
{
    setlocale(LC_ALL, "");
    ros::init(argc, argv, "demo_imu_data");

    ros::NodeHandle nh;
    ros::Subscriber sub = nh.subscribe("imu/data", 100, callback);
    ros::spin();

    return 0;
}